// 2025/3/4: 首个版本

using System;
using System.Collections.Generic;

namespace RemoteConsole
{
    class PointCloudSlicer
    {
        public byte[][] Slice(short[,] points, ushort[] intensities)
        {
            var pointCount = points.GetLength(0);
            var withI = intensities != null;
            var step = steps[Math.Max(0, Math.Min(19, (int)Math.Round(random.NextDouble() * 20 - 0.5)))];
            var sliceCount = Math.Max(1, (int)Math.Round((double)pointCount / SlicePointCountHint));

            var slots = new int[step];
            for (int i = 0; i < step; i++) slots[i] = i % sliceCount;

            var sliceIndices = new List<int>[sliceCount];
            for (int i = 0; i < sliceCount; i++) sliceIndices[i] = new List<int>();

            int k = 0;
            for (int i = 0; i < pointCount; i++)
            {
                sliceIndices[slots[k++]].Add(i);
                if (k == step) k = 0;
            }

            var slices = new byte[sliceCount][];
            for (int i = 0; i < sliceCount; i++)
            {
                var indices = sliceIndices[i];
                var slice = new byte[indices.Count * (withI ? 5 : 4)];
                int n = 0;
                foreach (var index in indices)
                {
                    short x = points[index, 0]; // cm
                    short y = points[index, 1]; // cm
                    short z = points[index, 2]; // cm

                    if (x < -10000 || x > 10000 || y < -10000 || y > 10000 || z < -150 || z > 1100) continue;

                    ushort xCode = (ushort)(2048 + (int)Math.Round((float)x / 5));
                    ushort yCode = (ushort)(2048 + (int)Math.Round((float)y / 5));
                    byte zCode = (byte)(32 + (int)Math.Round((float)z / 5));

                    slice[n++] = (byte)(xCode & 0x00ff);
                    slice[n++] = (byte)(((xCode & 0x0f00) >> 8) | ((yCode & 0x000f) << 4));
                    slice[n++] = (byte)((yCode & 0x0ff0) >> 4);
                    slice[n++] = zCode;

                    if (withI)
                    {
                        ushort s = intensities[index]; // 0~10000
                        slice[n++] = (byte)(s / 40);
                    }
                }
                slices[i] = n == 0 ? [] : (n == slice.Length ? slice : slice.AsSpan(0, n).ToArray());
            }
            return slices;
        }

        private Random random = new Random(DateTime.Now.ToString().GetHashCode());
        private int[] steps = [877, 881, 883, 887, 907, 911, 919, 929, 937, 941, 947, 953, 967, 971, 977, 983, 991, 997, 1009, 1013];

        private const int SlicePointCountHint = 4000;
    }
}